#ifndef AGV_UTIL__POINT_UTIL_HPP_
#define AGV_UTIL__POINT_UTIL_HPP_

#include <memory>
#include <geometry_msgs/msg/pose_stamped.hpp>
#include "geometry_msgs/msg/pose.hpp"
#include <iostream>
#include <fstream>
#include <string>
#include <vector>
#include <nlohmann/json.hpp>
#include "rclcpp/rclcpp.hpp"
// 自定义
#include "nav_msg_pkg/msg/waypoint.hpp"

using json = nlohmann::json;
namespace agv_util
{
    

    /**
     * @brief 点转换为pose
     */
    geometry_msgs::msg::PoseStamped point_to_pose(std::string pointName);

    // 加载点位
    std::vector<nav_msg_pkg::msg::Waypoint> load_waypoint();
    // 保存点位
    bool save_waypoint(std::vector<nav_msg_pkg::msg::Waypoint> &waypoints);


    // 定义结构体来表示每个节点及其邻居
    struct WaypointNeighbors
    {
        std::string name;
        std::vector<std::string> neighbors;
    };
    std::vector<WaypointNeighbors> load_waypoint_neighbors();
}

#endif // AGV_UTIL__POINT_UTIL_HPP_